from pyModbusTCP.client import ModbusClient
import struct


class Warehouse:
    TYPE_MATERIAL = 0
    TYPE_PRODUCT = 1

    class WarehouseLocation:
        def __init__(self, is_used, center, number_serial, number_hv):
            self.is_used = is_used
            self.number_serial = number_serial
            (self.number_horizontal, self.number_vertical) = number_hv
            self.center = center

        def get_is_used(self):
            return self.is_used

        def get_center(self):
            return self.center
            pass

        def get_number_serial(self):
            return self.number_serial

        def get_number_hv(self):
            return self.number_horizontal, self.number_vertical  # == (self.number_horizontal, self.number_vertical)

    def __init__(self, name, type,
                 count_horizontal, count_vertical,
                 length, width,
                 whloc_is_used=[]):
        self.name = name
        self.type = type

        self.count_horizontal = count_horizontal
        self.count_vertical = count_vertical

        self.length = length
        self.width = width

        self.warehouse_locations = []
        self.whloc_is_used = whloc_is_used

        for idx_h in range(0, self.count_horizontal):
            for idx_v in range(0, self.count_vertical):
                idx = idx_h * self.count_vertical + idx_v
                center = self.__left_up_hv(idx_h, idx_v) + self.__offset_hv()
                whloc = Warehouse.WarehouseLocation(self.whloc_is_used[idx], center,
                                                    idx, (idx_h, idx_v)
                                                    )
                self.warehouse_locations.append(whloc)
                pass

        pass

    def __len_per_unit_hv(self):

        return (self.length / self.count_horizontal,
                self.width / self.count_vertical)

    def __left_up_hv(self, idx_h, idx_v):
        (len_per_unit_h, len_per_unit_v) = self.__len_per_unit_hv()

        left_up_h = idx_h * len_per_unit_h
        left_up_v = idx_v * len_per_unit_v

        return (left_up_h, left_up_v)

    # matrix multiple format
    # def left_up_hv(self, idx_h, idx_v):
    #     return (idx_h, idx_v) * self.len_per_unit_hv()

    def __offset_hv(self):
        (len_per_unit_h, len_per_unit_v) = self.__len_per_unit_hv()

        return (len_per_unit_h / 2, len_per_unit_v / 2)

    def get_wh_location(self, is_used):
        for wh_loc in self.warehouse_locations:
            if wh_loc.is_used == is_used:
                return wh_loc
        pass

    pass


class RoboticArm:
    ACTION_NONE = 0
    ACTION_GET = 1
    ACTION_PUT = 2

    REGISTER_ADDR_ACTION_16BIT = 0x0000
    REGISTER_ADDR_X_H16BIT = 0x0002
    REGISTER_ADDR_X_L16BIT = 0x0003
    REGISTER_ADDR_Y_H16BIT = 0x0006
    REGISTER_ADDR_Y_L16BIT = 0x0007

    def __init__(self, hostip, port):
        self.connection = ModbusClient(host=hostip, port=port)
        if not self.connection.is_open:
            if not self.connection.open():
                info = "[ERROR] RoboticArm.__init__. can not connect to robotic arm"
                print(info)

    # float val -> hex val
    # online converter: https://www.binaryconvert.com/result_float.html?hexadecimal=43DB0000
    def __cvt_float_to_ieee754_hex(self, float_val):
        hex_string = struct.pack('>f',
                                 float_val).hex()  # float val(438) -> hex string(43db0000): https://blog.csdn.net/amzzsthl/article/details/132694854
        # print(result)
        # print(type(result))
        from_binary = 16
        hex_val_high16bits = int(hex_string[0:4],
                                 from_binary)  # hex string(43db) -> int val(17371==0x43db): https://blog.51cto.com/u_16175517/6761530
        hex_val_low16bits = int(hex_string[4:], from_binary)  # hex string(0000) -> int val(    0==0x0000)
        return hex_val_high16bits, hex_val_low16bits

    def __send_coord_val(self, c_addr, c_val_float):
        if not self.connection.is_open:
            info = "[ERROR] __send_coord_val. can not connect to robotic arm"
            print(info)
            return info

        (val_high16bits, val_low16bits) = self.__cvt_float_to_ieee754_hex(c_val_float)  # need cvt from float to hex
        (addr_high16bits, addr_low16bits) = c_addr

        res = self.connection.write_single_register(addr_high16bits, val_high16bits)
        res = self.connection.write_single_register(addr_low16bits, val_low16bits)

        return res

    def __send_xy(self, x, y):
        res = self.__send_coord_val(c_addr=(RoboticArm.REGISTER_ADDR_X_H16BIT, RoboticArm.REGISTER_ADDR_X_L16BIT),
                                    c_val_float=x)
        res = self.__send_coord_val(c_addr=(RoboticArm.REGISTER_ADDR_Y_H16BIT, RoboticArm.REGISTER_ADDR_Y_L16BIT),
                                    c_val_float=y)

        return res

    def __send_unknows(self):
        res = self.__send_coord_val(c_addr=(0x0004, 0x0005), c_val_float=0.)
        res = self.__send_coord_val(c_addr=(0x0008, 0x0009), c_val_float=0.)

        return res

    def __send_move_with_action(self, action_type):
        if not self.connection.is_open:
            info = "[ERROR] __send_move_with_action. can not connect to robotic arm"
            print(info)
            return info

        res = self.connection.write_single_register(RoboticArm.REGISTER_ADDR_ACTION_16BIT, action_type)

        return res

    def move_to_xyz_with_action(self, x, y, z, action_type):
        res = self.__send_xy(x, y)  # z is not need to be set
        res = self.__send_unknows()
        res = self.__send_move_with_action(action_type)

        return res


# NOTICE!!!
# suppose direction of axis x and y between 2d warehouse coordinate system and 3d robotic arm coordinate system are the same
# it should project if direction of axis x and y are not the same, including axis z
class CoordinateConverter:
    def __init__(self, wh_origin_3d_of_robotic_arm_coord_system):
        self.wh_origin_3d_of_robotic_arm_coord_system = wh_origin_3d_of_robotic_arm_coord_system

    def convert_to_3d_from_2d(self, coordinate_2d):
        coordinate_3d = self.wh_origin_3d_of_robotic_arm_coord_system + \
                        (coordinate_2d.x, coordinate_2d.y, 0)
        return coordinate_3d

    pass


class Gantry:
    COORDINATE_3D_CAR = (1, 1, 1)

    COORDINATE_3D_WARE_HOUSE_ORIGIN_MATERIAL = (2, 2, 2)
    COORDINATE_3D_WARE_HOUSE_ORIGIN_PRODUCT = (3, 3, 3)

    WAREHOUSE_MATERIAL_NUMBER_SERIAL_2_3D = [
        (438, 267, 0),  # {x, y, z}
        (638, 265, 0),
    ]
    WAREHOUSE_PRODUCT_NUMBER_SERIAL_2_3D = [
        (438, 41, 0),  # v1.0 NG
        (640, 41, 0),  #
    ]

    def __init__(self):
        self.robotic_arm = RoboticArm("192.168.1.16", 502)
        self.warehouse_material = Warehouse(name="M1", type=Warehouse.TYPE_MATERIAL,
                                            count_horizontal=1, count_vertical=2,
                                            length=200, width=100,
                                            whloc_is_used=[True, True])
        self.warehouse_product = Warehouse(name="P1", type=Warehouse.TYPE_PRODUCT,
                                           count_horizontal=1, count_vertical=2,
                                           length=200, width=100,
                                           whloc_is_used=[False, False])
        self.c_converter_material = CoordinateConverter(Gantry.COORDINATE_3D_WARE_HOUSE_ORIGIN_MATERIAL)
        self.c_converter_product = CoordinateConverter(Gantry.COORDINATE_3D_WARE_HOUSE_ORIGIN_PRODUCT)

    def get_material_from_warehouse_material(self):
        coordinate_2d = self.warehouse_material.get_wh_location(is_used=True).center()
        coordinate_3d = self.c_converter_material.convert_to_3d_from_2d(coordinate_2d)

        self.robotic_arm.move_to_xyz_with_action(coordinate_3d.x, coordinate_3d.y, coordinate_3d.z,
                                                 RoboticArm.ACTION_GET)
        pass

    # v1.0 should use this
    def get_material_from_warehouse_material_v1p0(self):
        wh_location = self.warehouse_material.get_wh_location(is_used=True)
        number_serial = wh_location.get_number_serial()
        coordinate_3d = Gantry.WAREHOUSE_MATERIAL_NUMBER_SERIAL_2_3D[number_serial+1]

        self.robotic_arm.move_to_xyz_with_action(coordinate_3d[0], coordinate_3d[1], coordinate_3d[2],
                                                 RoboticArm.ACTION_GET)
        pass

    def put_material_to_car(self):
        coordinate_3d = Gantry.COORDINATE_3D_CAR

        self.robotic_arm.move_to_xyz_with_action(coordinate_3d.x, coordinate_3d.y, coordinate_3d.z,
                                                 RoboticArm.ACTION_PUT)
        pass

    def get_product_from_car(self):
        coordinate_3d = Gantry.COORDINATE_3D_CAR

        self.robotic_arm.move_to_xyz_with_action(coordinate_3d.x, coordinate_3d.y, coordinate_3d.z,
                                                 RoboticArm.ACTION_GET)
        pass

    def put_product_to_warehouse_product(self):
        coordinate_2d = self.warehouse_product.get_wh_location(is_used=False).center()
        coordinate_3d = self.c_converter_product.convert_to_3D_from_2D(coordinate_2d)

        self.robotic_arm.move_to_xyz_with_action(coordinate_3d.x, coordinate_3d.y, coordinate_3d.z,
                                                 RoboticArm.ACTION_PUT)
        pass

    # v1.0 should use this
    def put_product_to_warehouse_product_v1p0(self):
        number_serial = self.warehouse_product.get_wh_location(is_used=False).get_number_serial()
        coordinate_3d = Gantry.WAREHOUSE_PRODUCT_NUMBER_SERIAL_2_3D[number_serial]

        self.robotic_arm.move_to_xyz_with_action(coordinate_3d[0], coordinate_3d[1], coordinate_3d[2],
                                                 RoboticArm.ACTION_PUT)
        pass

    pass


if __name__ == '__main__':
    gantry = Gantry()
    # gantry.get_material_from_warehouse_material_v1p0()
    gantry.put_product_to_warehouse_product_v1p0()
